/*
The MIT License (MIT)

Copyright (c) 2013 Mike Dapiran, Brian May, Richard Pospesel, and Bert Wierenga

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
//#include "../Hogshead.BulletPhysics.Enums.h"
#include "hhdPhysics.h"
#include "hhdMath.h"
#include <tinyxml.h>
#include "hhdStringBuilder.h"

namespace hhd
{
    namespace BulletPhysics
    {
        ComponentCPP(hhd::BulletPhysics::CollisionShape);
		HHD_REGISTRANT_CPP(IComponent, CollisionShape);

        CollisionShape::CollisionShape()
        {
            _collision_shape_types = (CollisionShapeType)0;
        }


        IComponent* CollisionShape::clone() const
        {
            CollisionShape* result = Factory::acquire<CollisionShape>();
            result->_collision_shape_types = _collision_shape_types;
            for(size_t k = 0; k < _raw_collision_data.size(); k++)
            {
                result->_raw_collision_data.add(_raw_collision_data[k]);
            }


            return result;
        }


        void CollisionShape::initialize()
        {

        }

        void CollisionShape::reset()
        {
            _raw_collision_data.clear();
            _collision_shape_types = (CollisionShapeType)0;
        }

        IComponent* CollisionShape::readXML(const String& in_xml)
        {
            IComponent::readXML(in_xml);

            TiXmlDocument document;
            document.Parse(in_xml);

            TiXmlElement* node = document.RootElement();


            // iterate over each node
            for(TiXmlNode* child = node->IterateChildren(0); child != NULL; child = node->IterateChildren(child))
            {
                if(String::equals(child->Value(), "Collider"))
                {

                    // string ot store the collision shape type"
                    String cst_string;

                    TiXmlElement* elem = child->ToElement();
                    if(elem->QueryStringAttribute("type", &cst_string) == TIXML_SUCCESS)
                    {
                        CollisionShapeType shape_type = (CollisionShapeType)0;
                        if(hhd::BulletPhysics::collision_shape_enum_table->tryGetValue(cst_string, shape_type))
                        {
                            _collision_shape_types = (CollisionShapeType)(_collision_shape_types | shape_type);
                            // common information all colliders will have for local transform
                            collision_shape_data shape_data;

                            elem->QueryVector3Attribute("position", &shape_data.position);
                            elem->QueryVector3Attribute("orientation", &shape_data.orientation);

                            shape_data.orientation.x = Math::degreesToRadians(shape_data.orientation.x);
                            shape_data.orientation.y = Math::degreesToRadians(shape_data.orientation.y);
                            shape_data.orientation.z = Math::degreesToRadians(shape_data.orientation.z);

                            shape_data.type = shape_type;
                            switch(shape_type)
                            {
                                /**
                                Sphere
                                	radius defaults to 1.0
                                **/
                            case Sphere:
                            {

                                float radius = 1.0f;
                                elem->QueryFloatAttribute("radius", &radius);
                                shape_data.data.sphere.radius = radius;
                                break;
                            }
                            /**
                            Plane
                            	by default, point up (0,1,0)
                            **/
                            case Plane:
                            {
                                Vector3 normal(0,1,0);
                                elem->QueryVector3Attribute("normal", &normal);

                                shape_data.data.plane.normal[0] = normal.x;
                                shape_data.data.plane.normal[1] = normal.y;
                                shape_data.data.plane.normal[2] = normal.z;
                                break;
                            }
                            /**
                            Box
                            	by default, half width (1,1,1)
                            	margin 0.04
                            **/
                            case Box3D:
                            {
                                Vector3 half_width(1,1,1);
                                elem->QueryVector3Attribute("halfWidth", &half_width);

                                shape_data.data.box3d.half_width[0] = half_width.x;
                                shape_data.data.box3d.half_width[1] = half_width.y;
                                shape_data.data.box3d.half_width[2] = half_width.z;
                                shape_data.margin = 0.04f;
                                break;
                            }
                            /**
                            Capsules
                            	by default: length 1 down the y axis, radius 1

                            **/
                            case CapsuleX:
                            case CapsuleY:
                            case CapsuleZ:
                            {
                                float height = 1.0f;
                                float radius = 1.0f;
                                elem->QueryFloatAttribute("height", &height);
                                elem->QueryFloatAttribute("radius", &radius);

                                shape_data.data.capsule.height = height;
                                shape_data.data.capsule.radius = radius;
                                shape_data.margin = 0.04f;
                                break;
                            }

                            /** Cones
                            	by default: height 1  radius 1
                            **/
                            case ConeX:
                            case ConeY:
                            case ConeZ:
                            {
                                float height = 1.0f;
                                float radius = 1.0f;
                                elem->QueryFloatAttribute("height", &height);
                                elem->QueryFloatAttribute("radius", &radius);

                                shape_data.data.cone.height = height;
                                shape_data.data.cone.radius = radius;
                                break;
                            }

                            /** Frustom **/
                            case Frustum:
                            {
                                shape_data.data.frustum.near_clip = 0.01f;
                                shape_data.data.frustum.far_clip = 1000.0f;
                                shape_data.data.frustum.horizontal_fov = 90.0f;
                                shape_data.data.frustum.aspect_ratio = 16.0f / 10.0f;

                                elem->QueryFloatAttribute("nearClip", &shape_data.data.frustum.near_clip);
                                elem->QueryFloatAttribute("farClip", &shape_data.data.frustum.far_clip);
                                elem->QueryFloatAttribute("fieldOfView", &shape_data.data.frustum.horizontal_fov);
                                elem->QueryFloatAttribute("aspectRatio", &shape_data.data.frustum.aspect_ratio);

                                shape_data.data.frustum.horizontal_fov = Math::degreesToRadians(shape_data.data.frustum.horizontal_fov);
                            }
                            }
                            elem->QueryFloatAttribute("margin", &shape_data.margin);
                            _raw_collision_data.add(shape_data);
                        }
                        else
                        {
							StringBuilder string_builder;
							String string;
							string_builder << "Could not parse Collider type \"" << cst_string << "\"";
							string_builder >> string;
                            hhd::error(string);
                        }
                    }
                }
            }
            if(_raw_collision_data.size() == 0)
            {
                error("CollisionShape does not have any Colliders in it");
            }

            return this;
        }

        btCollisionShape* CollisionShape::getBulletCollisionShape()
        {
            btCompoundShape* result = new btCompoundShape();

            for(size_t k = 0; k < _raw_collision_data.size(); k++)
            {
                collision_shape_data& csd = _raw_collision_data[k];
                btCollisionShape* bt_shape;
                switch(csd.type)
                {
                case Sphere:
                {
                    bt_shape = new btSphereShape(csd.data.sphere.radius);
                    break;
                }
                case Box3D:
                {
                    float* half_width = csd.data.box3d.half_width;
                    bt_shape = new btBoxShape(btVector3(half_width[0], half_width[1], half_width[2]));
                    break;
                }
                case Plane:
                {
                    float* normal = csd.data.plane.normal;
                    bt_shape = new btStaticPlaneShape(btVector3(normal[0], normal[1], normal[2]), 0.0f);
                    break;
                }
                case CapsuleX:
                {
                    bt_shape = new btCapsuleShapeX(csd.data.capsule.radius, csd.data.capsule.height);
                    break;
                }
                case CapsuleY:
                {
                    bt_shape = new btCapsuleShape(csd.data.capsule.radius, csd.data.capsule.height);
                    break;
                }
                case CapsuleZ:
                {
                    bt_shape = new btCapsuleShapeZ(csd.data.capsule.radius, csd.data.capsule.height);
                    break;
                }
                case ConeX:
                {
                    bt_shape = new btConeShapeX(csd.data.cone.radius, csd.data.cone.height);
                    break;
                }
                case ConeY:
                {
                    bt_shape = new btConeShape(csd.data.cone.radius, csd.data.cone.height);
                    break;
                }
                case ConeZ:
                {
                    bt_shape = new btConeShapeZ(csd.data.cone.radius, csd.data.cone.height);
                    break;
                }
                case Frustum:
                {
                    Matrix4 projection = Matrix4::createPerspectiveProjection(csd.data.frustum.horizontal_fov, csd.data.frustum.aspect_ratio, csd.data.frustum.near_clip, csd.data.frustum.far_clip);
                    Matrix4 projection_inverse;
                    projection.getSlowInverse(projection_inverse);

                    Vector3 near_point = (projection_inverse * Vector4(1.0f, 1.0f, 0.0, 1.0f)).project() * 0.5;
                    Vector3 far_point = (projection_inverse * Vector4(1.0f, 1.0f, 1.0, 1.0f)).project() * 0.5;


                    //hout << point << endl;

                    //float depth = (csd.data.frustum.far_clip - csd.data.frustum.near_clip);
                    /*
                    points[1].setValue(top_right.x,top_right.y, csd.data.frustum.far_clip);
                    points[2].setValue(top_right.x,-top_right.y, csd.data.frustum.far_clip);
                    points[3].setValue(-top_right.x,top_right.y, csd.data.frustum.far_clip);
                    points[4].setValue(-top_right.x,-top_right.y, csd.data.frustum.far_clip);
                    */
                    btConvexHullShape* bt_convex_hull = new btConvexHullShape();

                    /*
                    bt_convex_hull->addPoint(btVector3(near_point.x, near_point.y, near_point.z));
                    bt_convex_hull->addPoint(btVector3(-near_point.x, near_point.y, near_point.z));
                    bt_convex_hull->addPoint(btVector3(near_point.x, -near_point.y, near_point.z));
                    bt_convex_hull->addPoint(btVector3(-near_point.x, -near_point.y, near_point.z));
                    */

                    bt_convex_hull->addPoint(btVector3(0,0,0));

                    bt_convex_hull->addPoint(btVector3(far_point.x, far_point.y, far_point.z));
                    bt_convex_hull->addPoint(btVector3(-far_point.x, far_point.y, far_point.z));
                    bt_convex_hull->addPoint(btVector3(far_point.x, -far_point.y, far_point.z));
                    bt_convex_hull->addPoint(btVector3(-far_point.x, -far_point.y, far_point.z));

                    bt_shape = bt_convex_hull;
                    /*
                    btVector3* points = bt_convex_hull->getUnscaledPoints();
                    for(int k = 0; k < bt_convex_hull->getNumVertices(); k++)
                    	hout << points[k] << endl;
                    hout << endl;
                    */
                }
                }
                bt_shape->setMargin(csd.margin);
                btTransform local_transform(btQuaternion(csd.orientation.x, csd.orientation.y, csd.orientation.z), csd.position);
                result->addChildShape(local_transform, bt_shape);
            }
            return result;
        }

        void CollisionShape::deleteBtCollisionShape(btCollisionShape* in_shape)
        {
            if(btCompoundShape* compound_shape = dynamic_cast<btCompoundShape*>(in_shape))
            {
                int children = compound_shape->getNumChildShapes();
                for(int k = 0; k < children; k++)
                {
                    delete compound_shape->getChildShape(k);
                }
                delete compound_shape;
            }
            else
            {
                delete in_shape;
            }
        }

        bool CollisionShape::collision_shape_data::operator==(const  CollisionShape::collision_shape_data& right)
        {
            char* left_data = (char*)this;
            char* right_data = (char*)&right;
            for(size_t k = 0; k < sizeof(CollisionShape::collision_shape_data); k++)
            {
                if(left_data[k] != right_data[k])
                {
                    return false;
                }
            }
            return true;
        }
    }
}
